#include "pid_controller.h"
#include <algorithm>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <vector>
#include <thread>
#include <atomic>
#include <fstream>
#include <string>
#include "C_H.h"

// 位置式 PID 控制器的实现
float PIDController::calculate(float currentValue) {
    // 计算当前偏差
    error = setPoint - currentValue;
    
    // 计算积分项
    integral += error;
    
    // 积分限幅
    integral = std::max(-integralLimit, std::min(integral, integralLimit));
    
    // 计算微分项
    derivative = error - lastError;
        // 计算PID输出
    output = Kp * error +  kp* error *abs(error) + Ki * integral + Kd * derivative;// 0.045
    
    // 输出限幅
    output = std::max(-outputLimit, std::min(output, outputLimit));
    
    lastError = error;
    
    return output;
}

void PIDController::reset() {
    error = 0;
    lastError = 0;
    integral = 0;
    derivative = 0;
    output = 0;
}


float IncrementalPIDController::calculate(float currentValue, float setPoint) {
    float error = setPoint - currentValue; // 当前误差
    output += Kp * (error - lastError) + Ki * error; // 计算增量输出
    lastError = error; // 更新上一次误差

    // 添加限幅逻辑
    if (output > 6000) {
        output = 6000;
    } else if (output < -6000) {
        output = -6000;
    }
    return output; // 返回输出
}

void IncrementalPIDController::reset() {
    lastError = 0;
    output = 0;
} 